#pragma once 



class PidControl
{
private:
    /* data */
    float P;
    float I;
    float D;

    float Interr = 0.f;
    float LastErr = 0.f;

    float MaxOutput = 0.f;
    float MinOutput = 0.f;

public:
    PidControl(float _P, float _I, float _D, float _MaxOutput = 0.f, float _MinOutput = 0.f):
        P(_P), I(_I), D(_D), MaxOutput(_MaxOutput), MinOutput(_MinOutput) {};
    float operator()(float err) {
        float res = 0.0f;
        res += P * err;

        Interr += err;
        res += I * Interr;


        res += D * (err - LastErr);
        LastErr = err;

        if (MaxOutput > MinOutput) {
            if (res > MaxOutput) res = MaxOutput;
            else if (res < MinOutput) res = MinOutput;
        }

        return res;
    };

    void SetPID(float _P, float _I, float _D) {
        this->P = _P;
        this->I = _I;
        this->D = _D;
    }

    void SetRange(float _MaxOutput, float _MinOutput)
    {
        this->MaxOutput = _MaxOutput;
        this->MinOutput = _MinOutput;
    }
};



